#include <chrono>
#include <functional>
#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"
#include "common_lib/glog/glog.h"
#include <opencv2/opencv.hpp>
#include <Eigen/Eigen>
#include <Eigen/Dense>

// 参考 《视觉SLAM十四讲》第二版 6.3.1 章节
// ros2 run common_test test_fitting_curve_eigen

// xxty@xxty-virtual-machine:~/trobot/ros2_ws$ ros2 run common_test test_fitting_curve_eigen 
// iteration 0 cost=3.19575e+06 update: 0.0455771  0.078164 -0.985329 a=2.04558 b=-0.921836 c=4.01467
// iteration 1 cost=376785 update:  0.065762  0.224972 -0.962521 a=2.11134 b=-0.696864 c=3.05215
// iteration 2 cost=35673.6 update: -0.0670241   0.617616  -0.907497 a=2.04432 b=-0.0792484 c=2.14465
// iteration 3 cost=2195.01 update: -0.522767   1.19192 -0.756452 a=1.52155 b=1.11267 c=1.3882
// iteration 4 cost=174.853 update: -0.537502  0.909933 -0.386395 a=0.984045 b=2.0226 c=1.00181
// iteration 5 cost=102.78 update: -0.0919666   0.147331 -0.0573675 a=0.892079 b=2.16994 c=0.944438
// iteration 6 cost=101.937 update: -0.00117081  0.00196749 -0.00081055 a=0.890908 b=2.1719 c=0.943628
// iteration 7 cost=101.937 update:   3.4312e-06 -4.28555e-06  1.08348e-06 a=0.890912 b=2.1719 c=0.943629
// iteration 8 cost=101.937 update: -2.01204e-08  2.68928e-08 -7.86602e-09 a=0.890912 b=2.1719 c=0.943629
// cost: 101.937 >= last cost: 101.937, break!
// solve time cost = 0.000342864 seconds. 
// estimated a=0.890912 b=2.1719 c=0.943629

using namespace std;
using namespace Eigen;

class FittingCurveEigen : public rclcpp::Node{
    public:
        FittingCurveEigen();
        ~FittingCurveEigen() = default;

        // 拟合曲线
        void FittingCurve();
};


FittingCurveEigen::FittingCurveEigen(): Node("test_fitting_curve_eigen_node"){
    FittingCurve();
}

// 拟合曲线
void FittingCurveEigen::FittingCurve(){
    double ar=1.0, br=2.0, cr=1.0;      //真实参数值
    double ae=2.0, be=-1.0, ce=5.0;     //估计参数值
    int N=100;  //数据点个数
    double w_sigma=1.0;  //噪声Sigma值
    double inv_sigma=1.0/w_sigma;
    cv::RNG rng;        //opencv 随机数产生器

    // 构造数据集
    vector<double> x_data, y_data;
    for(int i=0; i<N; i++){
        double x = i/100.0;
        x_data.push_back(x);
        y_data.push_back(exp(ar*x*x + br*x + cr) + rng.gaussian(w_sigma*w_sigma));
    }

    // Gauss-Newton 高斯牛顿求解
    int iterations=100;             //迭代次数
    double cost=0, lastCost=0;      //本次迭代cost 和 上次迭代cost
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    for(int iter=0; iter<iterations; iter++){
        Matrix3d H = Matrix3d::Zero();      // Hessian = J^T * W^{-1} * J in Gauss-Newton  海塞矩阵
        Vector3d b = Vector3d::Zero();      // bias
        cost = 0;
        for(int i=0; i<N; i++){
            double xi = x_data[i];
            double yi = y_data[i];
            double error = yi - exp(ae*xi*xi + be*xi + ce);

            Vector3d J;  // Jacobian 雅可比矩阵
            J[0] = -xi * xi * exp(ae*xi*xi + be*xi + ce);   //de/da
            J[1] = -xi * exp(ae*xi*xi + be*xi + ce);        //de/db
            J[2] = -exp(ae*xi*xi + be*xi + ce);             //de/dc

            H += inv_sigma * inv_sigma * J * J.transpose();
            b += -inv_sigma * inv_sigma * error * J;

            cost += error * error;
        }

        // 求解线性方程 Hx = b
        Vector3d dx = H.ldlt().solve(b);
        if(isnan(dx[0])){
            cout << "result is nan!" << endl;
            break;
        }

        // 误差比上次大，迭代结束
        if(iter > 0 && cost >= lastCost){
            cout << "cost: " << cost << " >= last cost: " << lastCost << ", break!" << endl;
            break;
        }

        // 更新估计参数
        ae += dx[0];
        be += dx[1];
        ce += dx[2];

        lastCost = cost;
        cout << "iteration " << iter 
            << ", cost=" << cost 
            << ", update: " << dx.transpose()
            << ", a=" << ae << " b=" << be << " c=" << ce
            << endl;
    }
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2-t1);
    cout << "solve time cost = " << time_used.count() << " seconds. " << endl;
    cout << "estimated a=" << ae << " b=" << be << " c=" << ce << endl;
}




int main(int argc, char * argv[]){
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<FittingCurveEigen>());
    rclcpp::shutdown();
    return 0;
}